#ifndef HB_PID_H
#define HB_PID_H

#include <stdlib.h>
#include <cmath>
#include <stdint.h>


struct PIDInfo{
        float desired;
        float P;
        float I;
        float D;
        float FF;
        float AFF;
};



class PID {
public:

    PID(const float &   initialP = 0.0f,
        const float &   initialI = 0.0f,
        const float &   initialD = 0.0f,
        const int & initialImax = 0)
    {
        m_kp = initialP;
        m_ki = initialI;
        m_kd = initialD;
        m_imax = initialImax;

		// set _last_derivative as invalid when we startup
		m_lastDerivative = NAN;
    }

	/*
   	*** Iterate the PID, return the new control value
   	***
   	*** Positive error produces positive output.
   	***
   	*** @param error	The measured error value
 	*** @param scaler	An arbitrary scale factor
 	***
 	****  @returns		The updated control output.
 	***
  	*/
    float        getPid(float error, float scaler = 1.0);

    // Reset the PID integrator
    void        resetI();

    // Overload the function call operator to permit relatively easy initialisation
    void operator        () (const float    p,
                             const float    i,
                             const float    d,
                             const int  imaxval)
 	{
        m_kp = p;
		m_ki = i;
		m_kd = d;
		m_imax = imaxval;
    }

    float        getIntegrator() const {
        return m_integrator;
    }


	const struct PIDInfo& getPidInfo(void) const { return m_pidInfo; }

private:
    float       m_kp;
    float       m_ki;
    float       m_kd;
    int       	m_imax;

	// integrator value
    float           m_integrator;
	// last error for derivative
    float           m_lastError;
	/// last derivative for low-pass filter
    float           m_lastDerivative;
	/// last time get_pid() was called in millis
    int64_t        m_lastTime;
	
	PIDInfo m_pidInfo {};

 	/* Low pass filter cut frequency for derivative calculation.
  	***
  	*** 20 Hz because anything over that is probably noise, see
 	*** http://en.wikipedia.org/wiki/Low-pass_filter.
 	*/
    static const uint8_t        m_fCut = 20;
};


#endif //HB_PID_H